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. Abstract 

Human interaction introduces two main constraints: Safety 
and Comfort. Therefore service robot manipulator can't be 
controlled like industrial robotic manipulator where person- 
. nel is isolated from the robot's work envelope. In this paper, 
■ we present a soft motion trajectory planner to try to ensure 
that these constraints are satisfied. This planner can be 
used on-line to establish visual and force control loop suit- 
able in presence of human. The cubic trajectories build by 
this planner are good candidates as output of a manipula- 
. tion task planner. The obtained system is then homogeneous 
from task planning to robot control. 

The soft motion trajectory planner limits jerk, accelera- 
I tion and velocity in cartesian space using quaternion. Ex- 
- perimental results carried out on a Mitsubishi PA10-6CE 
arm are presented. 

1 INTRODUCTION 

Arm manipulator control for industrial applications has now 
reached a good level of maturity. Many solutions have been 
. proposed for specific utilizations. However, all these appli- 
' cations are confined to structured and safe spaces where no 
human-robot interactions occur. 

Arm manipulators for human interaction need to be in- 
trinsically safe [1] [2] , but the control level has also to guar- 
antee safety and comfort for humans. The soft motion tra- 
jectory planner presented in this paper provides tools to 
build such systems by limiting jerk, acceleration and veloc- 
ity. 

The problem of robot control has been divided in two hier- 
archical levels; the lower level called control or path tracking 
and the upper level called trajectory planning. Using this ap- 
proach, industrial robots can evolve at high speeds satisfying 
path constraints. Literature presents various works, Geering 
and al [3j propose time-optimal motions using a bang-bang 
control. Raj an proposes a two steps minimization algorithm 
[3], temporal/torque constraints are considered in the works 
of Shin and McKay [5j , Bobrow and al [6j and finally Kyri- 
akopoulos and Saridis propose minimal jerk control The 
objectives of the trajectory planner are to improve tracking 
accuracy and reduce manipulator wear by providing contin- 



uous references to the servo-motors control. Needs for pro- 
ductivity improvements for numerically controlled machine 
tools have generated numerous work to optimize feed-rate. 
In this case path tracking accuracy is far more important 
and approaches become similar. For example J. Dong [8] 
shows that limiting jerk in feed-rate optimization leads up 
a decrease of contouring errors and acoustic signals. 

In a human interaction context, safety is directly linked 
with the velocity limit and comfort with acceleration and 
jerk bounds. Such constrained movements are soft in carte- 
sian space even for rotations, starts and stops. This planner 
is used daily to plan trajectory along a path computed by 
HAMP [9j, a Human Aware Motion Planner, and Grasp 
Planner [lOj, both using MoveSD [TT]. These paths are de- 
fined by lines that connect different points. The tempo- 
ral evolution along the path is then computed by the soft 
motion trajectory planner as presented in the experimental 
section. Beside the limitations in jerk, acceleration and ve- 
locity provided by this approach, we hope that this would 
help to integrate visual and force loop that are known to 
have different time constant. 

This paper presents the related work in section II. Section 

III describes the soft motion trajectory planner and section 

IV presents some experimental results. 

2 Related Work 

To achieve smooth motion and tracking in task or joint 
space, several approaches have been presented, such as 
trapezoidal or bell-shaped velocity profiles using cubic, quar- 
tic or quintic polynomials. Lloyd [12j introduces a method 
adjusting the spatial shape of the transition curve of adja- 
cent path segments. Liu [13] uses seven cubics to update 
on-line a smooth mono-dimensional motion. 

Andersson [14J uses a single quintic polynomial for repre- 
senting the entire trajectory, while Macfarlane [15] extends 
Andersson's work and uses seven quintic polynomials for in- 
dustrial robots. 

In the case of human interaction Amirabdollahian and 
al [H] use a seventh order polynomial while Seki and 
Tadakuma \T7\ propose the use of fifth order polynomial, 
both of them for the entire trajectory with a minimum jerk 
model. Herrera and Sidobre [18j propose seven cubic equa- 



tions to obtain Soft Motions for robot service applications. 

3 Soft Motion Trajectory Planner 

We consider the planning of a trajectory defined by a set of 
points generated by path planning techniques that the end- 
effector must follow in cartesian space. We propose a soft 
motion trajectory planner that limits jerk, acceleration and 
velocity for service robot applications. 



at maximal jerk J max- The HIJ curve, in contrast, repre- 
sents minimal jerk evolution {—J max)- The acceleration axis 
becomes a symmetric axis of the two maximal and minimal 
jerk parabolas (eq. [2]&[3]). 



V{t) = + (2) 



3.1 Monodimensional Case 

In order to better understand elementary motions, we 
introduce the acceleration- velocity frame (Fig. [T]). Then 
we consider the point to point canonical case of Fig. [2l 
Finally, we extend our approach to general cases in which 
initial and final kinematic conditions are not null. 



3.1.1 The elementary motions in the Acceleration- 
Velocity frame 

Initial and final conditions are defined by: 

A(To)=Ao A(Tf) = Af 

V{n) = V^ V{Tf)=Vf (1) 

X(To)=Xo X{Tf) = Xf 

In order to simplify the presentation, we choose : 

Jmin — Jmax -^min — -^max ^min — ^max 

Curves J(t), V{t)^ X{t) respectively represent jerk, 

acceleration, velocity and position functions. In the Fig. 
m the point A corresponds to the state in which motion is 
stopped. Upper line JC and lower line EH respectively de- 
fine maximal (A^ax) and minimal {—A^ax) accelerations. 
The system can stay endlessly on a point along the IAD 
axis because of null acceleration. The velocity of motion 
is maximal {Vmax) on the point D and minimal {—Vmax) 
on /. The other states are unstable states, like for example 
from the point C, the only possible evolution is to join the 
point D. The CDE parabolic curve represents an evolution 
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where Vq G [-Vmax, Vmax] is the velocity at A{t) = 

The optimal motion is a motion with jerk, acceleration 
and velocity constraints successively saturated [18]. Then, 
we can define three elementary motions {Ai, Vi and Xi are 
initial conditions of segments) : 



The motion with a saturated jerk :^Jrr 
FA, AG, HI, IJ and KA segments): 

A(t) = Ai zb Jmaxt 



{AB, CD, DE, 



X{t) 



-■Xi 



Ajt dz nJm,axt 



Vd- 



i /t ./2 I 1 T 

2 '^i'^ ^ 6 ^ 



• The motion with a saturated acceleration ±Amax {BC, 
EF, GH and JK segments): 
J{t) = 

A(t) -^Amax 

V{t) =Vi± A^axt 

X{t)=Xi+Vit±\A^axt^ 



• Finally, the motion with a saturated velocity ±Vmax {D 
and / segments): 
J{t) = 
A(t) = 

V{t) = ±Vmax 
X{t) = Xi ± Vmaxt 



3.1.2 The point to point motion 

In this case, initial and final conditions are defined by: 



Figure 1: Acceleration- Velocity frame 



A(To) = A{Tf) = 
y(To) = ViTf) = 
X(To) = X{Tf) = Xf 
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Figure 2: Jerk, Acceleration, Speed and Position curves and 
Motion in the Acceleration- Velocity Frame 



of a constant velocity motion at —Vmax during dt2 > dt\. 
The motion at Vmax balances the motion at —Vmax- So, 
it's possible to find a motion with a shorter time which sat- 
isfies initial and final conditions. In other words, in the 
acceleration- velocity frame, a motion can't stay on both the 
D and / points. 

zz^ If a motion has a constant unsaturated velocity segment, 
it's also possible to find a motion with a saturated velocity 
segment or without a constant velocity segment at all. In 
both cases, the motion time is shorter. Thus, optimal mo- 
tions can't have an unsaturated constant velocity segment. 
Hi) If motion doesn't reach neither the D nor / points, 
parabolic curves can only be at the beginning or at end 
of motion. 

Therefore, optimal motions can't have more than seven ele- 
mentary motions. 

3.1.3 Types of motions 

As optimal motion is a Soft motion in minimal time, states 
with constant velocities can only be at the B and / points. 
So, in order to attain some initial and final conditions, there 
are two type of motions. A motion starting with a maxi- 
mum jerk segment will be called ty^e 1 motion and a motion 
starting with a minimum jerk segment, type 2 motion. For 
example. Fig. [3] illustrates type 1 motion which joins the 
point D. Fig. [4] illustrates type 2 motion which joins the 
point /. 

For short displacement, optimal motion doesn't have the 
constant velocity segment. However, we have to focus on a 
particular motion which we call Critical motion defined by 
a critical length dc. 



Fig. [2] represents the optimal motion which can be 
separated in seven segments: 
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Because of the point to point motion, it appears an anti- 
symmetry in acceleration and a symmetry in jerk with re- 
spect to the Tyc segment. Concerning the velocity curve, 
the symmetry effect is also present. We have then: 

Ti rri rri rri rri 

j — -'- jpoi — -J- jna — -J-jnb — -'- jpb 

TI rri rri rri rri 

a aca acb v vc 

Our system computes times Tj , and Ty to get the desired 
soft displacement between an origin position and a final po- 
sition. As the end effector moves under maximum motion 
conditions {Jmaxi ^max Vmax)i wc obtain a minimal time 
motion. However, optimal motion has seven elementary mo- 
tions at most as demonstrated below : 

i) We consider a motion composed of a constant velocity 
motion at Vmax^ which occurs during a period dti^ and also 



3.1.4 The critical length 

Fig. [5] presents critical motion that separates motion type 

1 (Fig. [3]) from motion type 2 (Fig. [4j). Critical length 
is the distance D = Xf — Xq done when the time motion 
is minimal and separates continiously motion type 1 and 
motion type 2. When the distance to cross becomes larger 
than dc^ motion is a type 1 motion. On the other hand, 
when the length becomes smaller than dc^ motion is a type 

2 motion. 

3.1.5 The general case 

The previous canonical case ()3.1.2p produces simple equa- 
tions because of the symmetry of curves. When initial and 
final kinematic conditions are no longer null, there is no more 
symmetry. However, it's important to observe that there is 
an impair symmetry between type 1 motion and type 2 mo- 
tion in the acceleration velocity frame. With this property, 
we can compute a type 2 motion as a type 1 motion and 
thus we can divide the number of algorithm's functions by 
two. We have developed an algorithm which computes the 
time of the seven segments for type 1 motions. Because of 
its size, we will not detail it in this paper. Inputs are ini- 
tial and final conditions (eq. [T]) and the Jmax^ ^max and 
Vmax constraints. This algorithm is based on thresholds 
that define particular lists of elementary motions. The most 
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Figure 3: Motion type 1 with Vmax reached 
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Figure 4: Motion type 2 with —Vmax reached 
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comphcated cases correspond to the resolution of a six de- 
gree equation. This equation represents the intersection of 
three parabohc curves. 

3.2 Multidimensional Case 

We present two interesting cases of the multidimensional 
extension: 

• The point to point motion: initial and final kinematic 
conditions are null. 

• The path following motion: the system has to pass over 
some points. 

3.2.1 The point to point motion 

Motion, in a n dimensional space between two points, is 
a straight-line path. The only way to ensure straight-line 
path is that motions have the same duration along each 
dimension. To do that, we compute the final time for each 
dimension. Considering the largest motion time, we readjust 
the other dimension motions to this time. Time adjusting 
is done by decreasing linearly J max, ^max and Vmax- In 
other words, the motion is minimum time for one direction. 
In the other directions, the motions are conditioned by the 
minimum one. 

3.2.2 The path following motion 

We consider a trajectory defined by points in the cartesian 
space (Fig. [6j). At least three points are necessary: the 
current position of the end-effector (PO), the first target po- 
sition (PI) and the final position (Pf). 
We describe the planification for a three points motion: 
Step 1: We compute the adjusted point to point motion 
(j3.2.ip between the current position (PO) and the interme- 
diate point (PI). We compute also the adjusted point to 
point motion between the point (PI) and the final point 
(Pf). In this state, the motion is stopped at (PI). 
Step 2: We use the algorithm described in 13.1.51 for each 
axis. For this transition motion, we use as initial conditions 
the ones found at the end point of the Tvc segment of the 
first point to point motion (ICt) (Fig. [6]) and as final con- 
ditions the states at the beginning of the Tvc segment of the 
second point to point motion {FCt)- So we have for each 
axis : 

A{ICt) = A{FCt) = 
V{ICt) = Vo V{FCt) = Vf 
X{ICt) = Xo X{FCt) = Xf 



Figure 5: Motion type hybrid: Critical Motion 



Step 3: Once the algorithm l3.1.5l is carried out, we have the 
optimal times Topt for each axis. Then, we have to constrain 
the motion time duration of each axis considering the axis 
which has the largest duration. We call this particular time 
T 

imp' 

For this transition motion, we can have various type 
of motions like start motion, stop motion and an infin- 
ity of combinations for Vb and Vf velocities varying in the 



[-Vmax, Vmax] interval. The length D = Xf - Xq\^ condi- 
tioned by Vq and Vf. Thus, this length is a particular one 
because we have computed the point to point motions at the 
beginning. It represents the distance done when the motion 
is hnking Vq and Vf passing over the point A (Fig. [Tj). So, 
adjusting time duration of the transition trajectory is more 
difficult than the adjustment of the point to point motion. 

However, we have a particular time T^top, the time needed 
to stop and restart the motion passing through (PI). If 
the imposed time Timp is larger than Tgtopi we can stop 
the motion and adjust the duration by adding time when 
the motion is stopped. In the other cases when Timp is 
between Topt and Tgtopi we have to find a combination of 
seven cubic segments satisfying initial and final conditions 
and the kinematic constraints. 

Now we are breaking down different ways to adjust the 
duration of axis transition motions. 

Soft transition motions must be under kinematic con- 
straints, so we can't increase Jmax-, ^max and Vmax- Because 
of real time constraint, we don't want to solve our problem 
by using random or optimization algorithms. In 13.2. we 
adjust duration by decreasing limit conditions {Jmax^ ^max 
and Vmax)' This strategy doesn't work anymore. Indeed, we 
can't decrease Vmax in a motion if initial and final velocities 
are Vmax- In this case, motion is only composed of a satu- 
rated velocity segment and decreasing Jmax or Amax doesn't 
change the duration of the motion. When initial and final 
velocities are smaller than Vmax, decreasing Jmax SiiidAmax 
increase the critical length. In this way, when critical length 
reaches and runs over the type of motion changes and a 
time interval without solution appears. So, we can't adjust 
motions like in 13.2.11 

Another solution is to find a seven cubic segments with a 
constant velocity Vc for the Tyc segment slower than Vmax 
which we call Slowing Velocity Motion. This motion isn't 
an optimal motion yet. However, there are intervals with no 
solution if the duration of the motion vary between Topt and 
Tstop- Indeed, in the cases of Vq and Vf are near Vmax, it's 
possible to don't have enough time to join the low velocity 
needed to do the distance D in a time Timp- The problem is 
that segments with saturated jerk last too long. Note that 
it's possible to minimize this problem by taking a jerk Jadj 




bigger than Jmax in order to decrease time of jerk saturated 
intervals. However, increase jerk is not a good solution be- 
cause motion run over kinematic constraints. 

For each axis, we compute intervals between Topt and Tgtop 
where a solution exists by computing Slowing Velocity Mo- 
tion. Then the imposed time Timp is the minimal time when 
there is a solution for each axis. An example illustrates this 
method in the Experimental Results part (|4.3p . 



4 Experimental Results 
4.1 Experimental Platform 

We implemented the soft motion trajectory planner on Jido 
(Fig. [7j), a mobile Neobotix platform MP-L655 with top 
mounted manipulator PAIO from Mitsubishi. The software 
control is developed using Open Robots tools: GenoM [19j . 
The sampling time is fixed to 10 ms. 

The linear and angular end-effector motions are limited 
by: 





Linear limits 


Angular limits 


Jmax 
Amax 
Vmax 


0.900m/s^ 
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0.200rad/s'^ 
O.lOOrad/s 



The Pose of the manipulator's end effector is defined 
by seven independent coordinates said Operational Coor- 
dinates. They give the position and the orientation of the 
final body in the reference frame. The advantages of using 
quaternions are largely exposed in [20] . 

We define P for the position and Q for the orientation 



[x y zy 



Q 



n q 



where 



[i j k] ' 



The linear obtained velocities V can be directly applied 
as velocity references. On another hand, the evolution of the 
quaternion Q must be transformed into angular velocities. 
We use the transformation function proposed in [21]. 
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Figure 6: Planning of a motion with three points 



Figure 7: Our robot Jido composed of a mobile base and a 
6 dof arm 




Figure 8: Soft motion of the end-effector (only X axis) 

4.2 On-line trajectory planning without 
time adjustment 

A 6 axis joystick (4 analog axis and 2 digital) gives velocity 
references (Vrc/) which must be followed by the end-effector. 

yRef = [^x vy vz cox coy coz] ^ 

To track these velocities, we use the trajectory planner 
()3.1.5p on-line. Translation motions are independently 
computed. However, we have to compute the quaternion 
derivative Q for angular motions. As the sampling time 
is 10 ms, we consider that angular variation is small. So, 
we can use the current quaternion Q as the final one to 
compute Q: 

Q = ^QQ with Q — [cox coy coz]^ 
Then, we have the vector VRefPose' 

T 

VRefPose = [VX Vy VZ Q] 

Trajectory is planned every 10 ms : initial conditions are the 
current state and final conditions are acceleration null and 
velocities VuefPose- For each direction, the distance to go 
D = Xf — XO is the critical length for initial and final con- 
ditions. This particular length defines the shortest motion 
to attain final conditions. Other lengths would introduce 
oscillations because motion will not directly join final con- 
ditions. Fig. [8] illustrates the end-effector evolution for X 
axis. 

4.3 Tracking trajectory motion 

Even though we can do rotation, for the clarity of the pre- 
sentation, we present a translation motion. We consider the 
trajectory defined by the three points : 
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Figure 9: Time intervals where Slowing Velocity Motion 
works (0 : without solution ; 1 : with solution) 

Considering steps and notation explained on l3.2.2[ we com- 
pute the point to point motions between PO and PI and 
between PI and Pf. So, the initial and final conditions for 
the transition motion are : 





Axis X 


Axis Y 


Axis Z 


V(/Ct) (m/s) 


0.150 


0.150 





V(FCt) (m/s) 


0.150 


0.150 


0.15 


D (m) 


0.125 


0.125 


0.0623 


Topt (s) 


0.833 


0.833 


0.84 



where D is the axis displacement. At this step, we have to 
adjust transition motion times. So, as explained in I3.2.2[ 
we compute time intervals when Slowing Velocity Motion 
works. Fig. [9] illustrates how to find Timp • 

More video results could be found at: 
http://www.laas.fr/r^xhroquer 

5 Conclusions 

The soft motion trajectory planner presented in this paper 
is simpler than previous ones and avoids the optimization 
stage. For both the point to point motion and the transition 
motion, series of cubic curves are computed. For each axis, 
these cubic trajectories share the same time intervals. Due 
to direct computation of cubic parameters, the planner is 
fast enough to be used on-line. 

Experimental results show the validity of the approach 
for real-time control and trajectory planning in human pres- 
ence. To improve task planner characteristics, we are cur- 
rently incorporating the trajectory planner into the path 
planner. Our objective is to directly build soft cubic curves 
at the task planification level and enjoy richer families of 
curves. 
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